#include <stdio.h>
#include <inttypes.h>
#include <math.h>
#include "color_converter.h"

static inline uint8_t clamp(float value) {
    if (value < 0) return 0;
    if (value > 255) return 255;
    return (uint8_t)value;
}
// YUV的标准挺多的,这个是ITU-R Recommendation BT.601标准
// 参考https://www.programmingalgorithms.com/algorithm/yuv-to-rgb/c/
void yuv2rgb(uint8_t y, uint8_t u, uint8_t v, uint8_t *r, uint8_t *g, uint8_t *b) 
{
    float R = y + 1.4075f * (v - 128);
	float G = y - 0.3455f * (u - 128) - (0.7169f * (v - 128));
	float B = y + 1.7790f * (u - 128);
    *r = clamp(R);
    *g = clamp(G);
    *b = clamp(B);
}
// 参考https://www.programmingalgorithms.com/algorithm/rgb-to-yuv/c/
void rgb2yuv(uint8_t r, uint8_t g, uint8_t b, uint8_t *y, uint8_t *u, uint8_t *v)
{
    float Y = r * 0.299000f + g * 0.587000f + b * 0.114000f;
	float U = r * -0.168736f + g * -0.331264f + b * 0.500000 + 128;
	float V = r * 0.500000f + g * -0.418688f + b * -0.081312f + 128;
    *y = clamp(Y);
    *u = clamp(U);
    *v = clamp(V);
}

// 参考https://blog.csdn.net/songyulong8888/article/details/117228659
#define max(a,b) ((a) > (b) ? (a) : (b))
#define min(a,b) ((a) < (b) ? (a) : (b))
#define max3(a,b,c) (((a) > (b) ? (a) : (b)) > (c) ? ((a) > (b) ? (a) : (b)) : (c))
#define min3(a,b,c) (((a) < (b) ? (a) : (b)) < (c) ? ((a) < (b) ? (a) : (b)) : (c))
void hsv2rgb(float h, float s, float v, uint8_t *rgb_r, uint8_t *rgb_g, uint8_t *rgb_b) 
{
	int i;
    float f,a,b,c;
	if (h >= 360) {
		h = 0;
	}
    if (s == 0) {
        *rgb_r = (uint8_t)(v*255);
        *rgb_g = (uint8_t)(v*255);
        *rgb_b = (uint8_t)(v*255);
    } else {
        h /= 60.0;  // sector 0 to 5, h_max=360 360/60=6[0,1,2,3,4,5]
        i = (int)floor(h); // floor(h)
        f = h-i; // factorial path of h
        a = v * (1-s);
        b = v * (1-s*f);
        c = v * (1-s*(1-f));
        switch(i) 
        {
            case 0:
                *rgb_r = (uint8_t)(v*255);
                *rgb_g = (uint8_t)(c*255);
                *rgb_b = (uint8_t)(a*255);
                break;
            case 1:
                *rgb_r = (uint8_t)(b*255);
                *rgb_g = (uint8_t)(v*255);
                *rgb_b = (uint8_t)(a*255);
                break;
            case 2:
                *rgb_r = (uint8_t)(a*255);
                *rgb_g = (uint8_t)(v*255);
                *rgb_b = (uint8_t)(c*255);
                break;
            case 3:
                *rgb_r = (uint8_t)(a*255);
                *rgb_g = (uint8_t)(b*255);
                *rgb_b = (uint8_t)(v*255);
                break;
            case 4:
                *rgb_r = (uint8_t)(c*255);
                *rgb_g = (uint8_t)(a*255);
                *rgb_b = (uint8_t)(v*255);
                break;
            default:
                *rgb_r = (uint8_t)(v*255);
                *rgb_g = (uint8_t)(a*255);
                *rgb_b = (uint8_t)(b*255);
                break;
        }
    }
}
void rgb2hsv(uint8_t u8_r, uint8_t u8_g, uint8_t u8_b, float *h, float *s, float *v)
{
    float max, min, delta=0;
    float r = u8_r / 255.0f;
    float g = u8_g / 255.0f;
    float b = u8_b / 255.0f;
    max = max3(r, g, b);
    min = min3(r, g, b);
    delta = (max - min);
    if (delta == 0) {
        *h = 0;
    } else {
        if (r == max) {
            *h = ((g-b)/delta)*60; 
        } else if (g == max) {
            *h = 120+(((b-r)/delta)*60); 
        } else if (b == max) {
            *h = 240 + (((r-g)/delta)*60);
        }
        if (*h < 0) {
            *h += 360;
        }
    }
    if (max == 0) {
        *s = 0;
    } else {
        *s = delta/max;
    }
    *v = max;
}

#if 0
// 参考https://blog.csdn.net/lz0499/article/details/77345166
// RGB2Lab Lab2RGB
const float param_13 = 1.0f / 3.0f;
const float param_16116 = 16.0f / 116.0f;
const float Xn = 0.950456f;
const float Yn = 1.0f;
const float Zn = 1.088754f;

static float gamma_RGB2XYZ(float x)
{
    return x > 0.04045f ? powf((x + 0.055f) / 1.055f, 2.4f) : (x / 12.92f);
};

static void RGB2XYZ(uint8_t R, uint8_t G, uint8_t B, float *X, float *Y, float *Z)
{
    float RR = gamma_RGB2XYZ(R / 255.0f);
    float GG = gamma_RGB2XYZ(G / 255.0f);
    float BB = gamma_RGB2XYZ(B / 255.0f);

    *X = 0.4124564f * RR + 0.3575761f * GG + 0.1804375f * BB;
    *Y = 0.2126729f * RR + 0.7151522f * GG + 0.0721750f * BB;
    *Z = 0.0193339f * RR + 0.1191920f * GG + 0.9503041f * BB;
}

static void XYZ2Lab(float X, float Y, float Z, float *L, float *a, float *b)
{
    float fX, fY, fZ;

    X /= (Xn);
    Y /= (Yn);
    Z /= (Zn);

    if (Y > 0.008856f)
        fY = pow(Y, param_13);
    else
        fY = 7.787f * Y + param_16116;

    if (X > 0.008856f)
        fX = pow(X, param_13);
    else
        fX = 7.787f * X + param_16116;

    if (Z > 0.008856)
        fZ = pow(Z, param_13);
    else
        fZ = 7.787f * Z + param_16116;

    *L = 116.0f * fY - 16.0f;
    *L = *L > 0.0f ? *L : 0.0f;
    *a = 500.0f * (fX - fY);
    *b = 200.0f * (fY - fZ);
}

static inline float gamma_XYZ2RGB(float x)
{
    return x > 0.0031308f ? (1.055f * powf(x, (1 / 2.4f)) - 0.055f) : (x * 12.92f);
};

static void XYZ2RGB(float X, float Y, float Z, uint8_t *R, uint8_t *G, uint8_t *B)
{
    float RR, GG, BB;
    RR = 3.2404542f * X - 1.5371385f * Y - 0.4985314f * Z;
    GG = -0.9692660f * X + 1.8760108f * Y + 0.0415560f * Z;
    BB = 0.0556434f * X - 0.2040259f * Y + 1.0572252f * Z;

    RR = gamma_XYZ2RGB(RR);
    GG = gamma_XYZ2RGB(GG);
    BB = gamma_XYZ2RGB(BB);

    RR = clamp(RR * 255.0f + 0.5f);
    GG = clamp(GG * 255.0f + 0.5f);
    BB = clamp(BB * 255.0f + 0.5f);

    *R = (uint8_t)RR;
    *G = (uint8_t)GG;
    *B = (uint8_t)BB;
}

static void Lab2XYZ(float L, float a, float b, float *X, float *Y, float *Z)
{
    float fX, fY, fZ;

    fY = (L + 16.0f) / 116.0f;
    fX = a / 500.0f + fY;
    fZ = fY - b / 200.0f;

    if (powf(fY, 3.0f) > 0.008856f)
        *Y = powf(fY, 3.0f);
    else
        *Y = (fY - param_16116) / 7.787f;

    if (powf(fX, 3.0f) > 0.008856f)
        *X = fX * fX * fX;
    else
        *X = (fX - param_16116) / 7.787f;

    if (powf(fZ, 3.0f) > 0.008856f)
        *Z = fZ * fZ * fZ;
    else
        *Z = (fZ - param_16116) / 7.787f;

    (*X) *= (Xn);
    (*Y) *= (Yn);
    (*Z) *= (Zn);
}

void rgb2lab(uint8_t R, uint8_t G, uint8_t B, float *L, float *a, float *b) 
{
    float X,Y,Z;
    RGB2XYZ(R, G, B, &X, &Y, &Z);
    XYZ2Lab(X, Y, Z, L, a, b);
}

void lab2rgb(float L, float a, float b, uint8_t *R, uint8_t *G, uint8_t *B) 
{
    float X,Y,Z;
    Lab2XYZ(L, a, b, &X, &Y, &Z);
    XYZ2RGB(X, Y, Z, R, G, B);
}
#endif